#include "WPILib.h"

#include <iostream.h>
#include "math.h"
#include <taskLib.h>
#include "string.h"
// #include "DriverStationLCD.h"
#include "SpeedController.h"

#include "vxWorks.h" 
#include "TrackAPI.h" 
#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "PCVideoServer.h"
#include "SimpleRobot.h"
// #include "Target.h"

/*for us, 
 AnalogChannel ac(1);
 ac.GetValue();
 */

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class Default2489Robot : public SimpleRobot {

	AnalogChannel *xaccel;
	AnalogChannel *yaccel;
	RobotDrive *myRobot; // robot drive system
	//RobotDrive *Turret;
	Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick; // joystick 2 (tank left stick)
	Joystick *manip;

	DriverStation *ds; // driver station object
	Encoder *rightencoder;
	Encoder *leftencoder;
	//Relay *compressor;

	Accelerometer *xaccelerometer;
	Accelerometer *yaccelerometer;

	Jaguar *Turret;
	Relay *Sweeper;
	Jaguar *Screw;
	Jaguar *Shooter;
	Jaguar *Sweeper2;

	// Declare pointers to Compressor and Solenoid objects
	Compressor *m_compressor; // air compressor
	Solenoid *m_solenoidball; // pneumatics solenoid
	Solenoid *m_solenoidballb;
	Solenoid *m_solenoidcatcha;
	Solenoid *m_solenoidcatchb;
	Solenoid *test1; //test variable
	Solenoid *test2;
	// Declare pointers to Gyro 
	DigitalInput *leftlim;
	DigitalInput *rightlim;
	DigitalModule *module;
	//DashboardDataFormat dashboardDataFormat;
	float horizontalDestination; // servo destination (0.0-1.0)
	float verticalDestination; // servo destination (0.0-1.0)
	//HiTechnicAccelerometer *accel;     //accel
	float distancea;
	float velocitya;
	float rmotor;
	float lmotor;
	//int framesPerSecond; // number of camera frames to get per second
	//double sinStart; // control where to start the sine wave input for pan
	//ParticleAnalysisReport par1, par2; // particle analysis reports
	//TrackingThreshold td1, td2; // color thresholds
	// To locally enable debug printing: set the debugFlag to a 1, to disable set to 0

#define PI 3.14159265358979

	// for 160x120, 50 pixels = .38 %
	//#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
	//#define MAX_PARTICLE_TO_IMAGE_PERCENT 20.0		// target is too close
	/** Simple test to see if the color is taking up too much of the image */
	//int tooClose(ParticleAnalysisReport* par) {
	//if (par->particleToImagePercent > MAX_PARTICLE_TO_IMAGE_PERCENT)
	//return 1;
	//return 0;
	//}

	/** Simple test to see if the color is large enough */
	//int bigEnough(ParticleAnalysisReport* par) {
	//if (par->particleToImagePercent < MIN_PARTICLE_TO_IMAGE_PERCENT)
	//return 0;
	//	return 1;
	//}
	enum // Driver Station jumpers to control program operation
	{	corner = 1,// Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
		ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
	} jumpers;

public:
	/**
	 * 
	 * 
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	Default2489Robot(void) {
		//xaccel = new AnalogChannel(2,1);
		//yaccel = new AnalogChannel(2,2);
		ds = DriverStation::GetInstance();
		myRobot = new RobotDrive(2, 1); // create robot drive base
		rightStick = new Joystick(1); // create the joysticks
		leftStick = new Joystick(2);
		manip = new Joystick(3);

		Turret = new Jaguar(4,5);
		Sweeper = new Relay(4,1);
		Screw = new Jaguar(4,3);
		Sweeper2 = new Jaguar(4,8);

		Shooter = new Jaguar(4,4);
		//Turret = new RobotDrive(3,4);
		leftencoder = new Encoder(1,2);
		rightencoder = new Encoder(3,4);

		leftencoder->SetReverseDirection(1);

		xaccelerometer = new Accelerometer(2,8);
		yaccelerometer = new Accelerometer (2,7);
		xaccelerometer->SetZero(1.5);
		yaccelerometer->SetZero(1.5);

		// Define compressor object with 
		//		pressure switch on digital input 5
		//		compressor relay on relay output 1
		m_compressor = new Compressor(5,5);
		m_compressor->Start();

		// Define pneumatics solenoid object on solenoid output 1
		m_solenoidball = new Solenoid(1);
		m_solenoidballb = new Solenoid(2);
		m_solenoidcatcha = new Solenoid (3);
		m_solenoidcatchb = new Solenoid (4);
		//setup test Solenoids
		test1 = new Solenoid(5);
		test2 = new Solenoid(6);
		//define camera limits
		leftlim = new DigitalInput(4,7);
		rightlim = new DigitalInput(4,8);

		GetWatchdog().SetExpiration(0.1); //Update the motors at least every 100ms.
	}

	void RobotInit(void) {
		//m_compressor->Start();
		rightencoder->Reset();
		leftencoder->Reset();
		//DigitalModule *dm = DigitalModule::GetInstance(4); // 4 is cRIO slot #
		rmotor=0.;
		lmotor=0.;
		leftencoder->SetDistancePerPulse(.13343154);
		rightencoder->SetDistancePerPulse(.1334314);
		leftencoder->Start();
		rightencoder ->Start();

	}

	void Autonomous(void) {
		double t = 0.0;
		double time_running = 14.5;
		double s = 0.0;
		double s2 = 0.0;
		GetWatchdog().SetEnabled(true);

		while (IsAutonomous()) {

			for (t = 0.0; t < time_running; t+=0.01) {
				if (!IsAutonomous()) {
					break;
				}

				if (t < 6.0) {
					if (s < 1.) {
						s+=0.01; 
						s2+=0.01;
					}
				}

				else {
					s = 1.0;
					s2 = -0.7;
				}

				GetWatchdog().Feed();
				myRobot->SetLeftRightMotorSpeeds(s, s2);
				Wait(0.01);

			}

		}

	}

	


	void OperatorControl(void) {
		//	m_compressor->Start();
		//myRobot->Drive(0.,0.);

		while (IsOperatorControl()) {

			GetWatchdog().Feed();
			/* will set joystick so that it can only accelerate, not jump
			 * theory comes from getting the PWM and adding every so second. Increasing joystick value which goes from 0-1 will
			 * shorten that time thus causing greater acceleration
			 */

			// determine if tank or arcade mode; default with no jumper is for tank drive

			/* will set joystick so that it can only accelerate, not jump
			 * theory comes from getting the PWM and adding every so second. Increasing joystick value which goes from 0-1 will
			 * shorten that time thus causing greater acceleration
			 */

			// determine if tank or arcade mode; default with no jumper is for tank drive
			if (leftStick->GetZ()>0&& leftStick->GetTrigger()) {
				myRobot->TankDrive(rightStick, leftStick); // drive with tank style
				
				rmotor = rightStick->GetY();
				lmotor = leftStick->GetY();
			}
			if (leftStick->GetZ()<0) {
				myRobot->ArcadeDrive(leftStick); // drive with arcade style (use left stick)
			}
			if (leftStick->GetZ()>0&&!leftStick->GetTrigger()) {

				//right joystick
				//this will control the overall motor

				float rjoystickconst = .020;
				rmotor = rmotor + rjoystickconst*rightStick->GetY();
				if (rightStick->GetRawButton(3)) {
					rmotor=0.;
				}
				if (rmotor>1.) {
					rmotor= 1.;
				}

				if (rmotor<-1.) {
					rmotor = -1.;
				}
				if (rmotor>0.&&rightStick->GetY()==0.) {
					rmotor = rmotor - .020;
				}
				if (rmotor<0.&&rightStick->GetY()==0.) {
					rmotor = rmotor + .020;
				}

				//left joystick variable

				float ljoystickconst = .028;
				lmotor = lmotor + ljoystickconst*leftStick->GetY();
				if (lmotor>1.) {
					lmotor= 1.;
				}

				if (lmotor<-1.) {
					lmotor = -1.;
				}
				if (leftStick->GetRawButton(3)) {
					lmotor = 0.;
				}
				if (lmotor>0.&&leftStick->GetY()==0.) {
					lmotor = lmotor - .028;
				}
				if (lmotor<0.&&leftStick->GetY()==0.) {
					lmotor = lmotor + .028;
				}
				myRobot->SetLeftRightMotorSpeeds(rmotor, lmotor);
			}
			if (!leftStick->GetTrigger()==1||!rightStick->GetTrigger()==1
					||!manip->GetTrigger()==1||!rightStick->GetRawButton(11)
					||!rightStick->GetRawButton(12)) {
				Screw->Set(0.);
			}
			if (leftStick->GetRawButton(7)==1||leftStick->GetRawButton(7)) {
				Sweeper->Set(Relay::kOff);
			}
			//give both the ability to use the screw. reason, Turret use needs screw at same time. Same with sweeper
			if (leftStick->GetTrigger()==1||rightStick->GetTrigger()==1
					||manip->GetTrigger()==1) {
				Screw->Set(1.0);
			}
			//give manipulator driver control of sweeper
			if (!manip->GetTrigger()) {
				Shooter->Set(0.0);
			}
			if (manip->GetTrigger()) {
				Shooter->Set(1.0);
			}

			if (rightStick->GetRawButton(11)||rightStick->GetRawButton(11)) {
				Screw->Set(-1.0);
			}
			if (rightStick->GetRawButton(10)||leftStick->GetRawButton(10)) {
				Screw->Set(1.0);
			}
			if (leftStick->GetTrigger()==1||rightStick->GetTrigger()==1
					&&rightStick->GetRawButton(6)||leftStick->GetRawButton(6)) {
				Sweeper->Set(Relay::kOn);
				Sweeper->SetDirection(Relay::kForwardOnly);
			}

			if (!manip->GetRawButton(4)&&!manip->GetRawButton(5)) {
				Turret->Set(0.);
			}
			if (manip->GetRawButton(4)) {
				Turret->Set(1.0);
			}
			if (manip->GetRawButton(5)) {
				Turret->Set(-1.0);
			} 

			Sweeper2->Set(manip->GetZ());

			Wait(.001);

		}
	}

};

START_ROBOT_CLASS(Default2489Robot);
